#include "JTSData.h"
#include "JLog.h"
#include "JProject.h"

#include <QFile>
#include <QStringList>
#include <QDialog>
#include <QGridLayout>
#include <QHeaderView>
#include <cmath>

#define PI 3.141592653589793
#define R2D (180.0/PI)

JTSData::JTSData(const QString& filename)
{
	/*QFile file(filename);
	if ( file.open( QIODevice::ReadOnly | QIODevice::Text ) )
	{
		int nRow = 0;
		while(!file.atEnd())
			++nRow;
		m_nSize = nRow;
	}
	file.close();*/
	loadFromFile(filename);
}

JTSData::~JTSData()
{

}

void JTSData::loadFromFile( const QString& filename )
{
	QFile file(filename);
		
	if ( file.open( QIODevice::ReadOnly | QIODevice::Text ) ) 
	{
		int nRow = 0;
		while ( !file.atEnd() ) 
		{
			QByteArray line = file.readLine();
			QStringList vals = QString(line).split(','/*QRegExp(" *, *")*/, QString::SkipEmptyParts);
			if (nRow == 0)
			{
				Log.Post(vals[nRow]);
				if (vals[nRow].contains("JT_EULER_312"))
				{
					m_Format = JT_EULER_312;
				}
			}
			if (nRow >= 2)
			{
				PoseEntry entry;
				entry.trans.setX(vals[0].toFloat());
				entry.trans.setY(vals[1].toFloat());
				entry.trans.setZ(vals[2].toFloat());

				switch(m_Format)
				{
				case JT_EULER_312:
					entry.rot.setZ(vals[3].toFloat());
					entry.rot.setX(vals[4].toFloat());
					entry.rot.setY(vals[5].toFloat());

					float rotation[4];
					toQuaternion312(vals[3].toFloat(),vals[4].toFloat(),vals[5].toFloat(), rotation);
					entry.quat.setScalar(rotation[0]);
					entry.quat.setX(rotation[1]);
					entry.quat.setY(rotation[2]);
					entry.quat.setZ(rotation[3]);

					break;
				default:;
				}
				m_Data.append(entry);
			}
			nRow++;
		}
		Log.Post("JTS File Loaded: "+file.fileName(), JLOG_SUCCESS);
	}
	file.close();
}

int JTSData::size()
{
	return m_Data.size();
}

const PoseEntry& JTSData::data( int index )
{
	return m_Data.at(index);
}

void JTSData::display()
{
	if(!m_pTable) m_pTable = new QTableWidget(NULL);
	m_pTable->setRowCount(m_Data.size());
	m_pTable->setColumnCount(10);
	m_pTable->setEditTriggers(QAbstractItemView::NoEditTriggers);

	//QTableWidget table(m_Data.size(),10);
	QStringList headers;
	switch (m_Format)
	{
	case JT_EULER_312:
		m_pTable->setWindowTitle("JT_EULER_312");
		headers << "x_tran"<< "y_tran"<< "z_tran"<< "z_rot"<< "x_rot"<< "y_rot"<< "q0"<< "q1"<< "q2"<< "q3";
		break;
	default:
		headers << "x_tran"<< "y_tran"<< "z_tran"<< "x_rot"<< "y_rot"<< "z_rot"<< "q0"<< "q1"<< "q2"<< "q3";
	}
	m_pTable->setHorizontalHeaderLabels(headers);

	QStringList rows;

	QTableWidgetItem* xtran;
	QTableWidgetItem* ytran;
	QTableWidgetItem* ztran;
	QTableWidgetItem* xrot;
	QTableWidgetItem* yrot;
	QTableWidgetItem* zrot;
	QTableWidgetItem* q0;
	QTableWidgetItem* q1;
	QTableWidgetItem* q2;
	QTableWidgetItem* q3;

	for (int i = 0; i < m_Data.size()+1; ++i)
	{
		rows.append(QString::number(i+1));
		
		xtran = new QTableWidgetItem(QString::number(m_Data.at(i).trans.x()));
		//xtran.setText(QString::number(m_Data.at(i).trans.x()));	
		m_pTable->setItem(i,0,xtran);
		ytran = new QTableWidgetItem(QString::number(m_Data.at(i).trans.y()));
		//ytran.setText(QString::number(m_Data.at(i).trans.y()));
		m_pTable->setItem(i,1,ytran);
		ztran = new QTableWidgetItem(QString::number(m_Data.at(i).trans.z()));
		//ztran.setText(QString::number(m_Data.at(i).trans.z()));
		m_pTable->setItem(i,2,ztran);
		switch(m_Format)
		{
		case JT_EULER_312:
			zrot = new QTableWidgetItem(QString::number(m_Data.at(i).rot.z()));
			//zrot.setText(QString::number(m_Data.at(i).rot.z()));
			m_pTable->setItem(i,3,zrot);
			xrot = new QTableWidgetItem(QString::number(m_Data.at(i).rot.x()));
			//xrot.setText(QString::number(m_Data.at(i).rot.x()));
			m_pTable->setItem(i,4,xrot);
			yrot = new QTableWidgetItem(QString::number(m_Data.at(i).rot.y()));
			//yrot.setText(QString::number(m_Data.at(i).rot.y()));
			m_pTable->setItem(i,5,yrot);
			break;
		default:
			xrot = new QTableWidgetItem(QString::number(m_Data.at(i).rot.x()));
			//xrot.setText(QString::number(m_Data.at(i).rot.x()));
			m_pTable->setItem(i,3,xrot);
			yrot = new QTableWidgetItem(QString::number(m_Data.at(i).rot.y()));
			//yrot.setText(QString::number(m_Data.at(i).rot.y()));
			m_pTable->setItem(i,4,yrot);
			zrot = new QTableWidgetItem(QString::number(m_Data.at(i).rot.z()));
			//zrot.setText(QString::number(m_Data.at(i).rot.z()));
			m_pTable->setItem(i,5,zrot);
		}
		q0 = new QTableWidgetItem(QString::number(m_Data.at(i).quat.scalar()));
		//q0.setText(QString::number(m_Data.at(i).quat.scalar()));
		m_pTable->setItem(i,6,q0);
		q1 = new QTableWidgetItem(QString::number(m_Data.at(i).quat.x()));
		//q1.setText(QString::number(m_Data.at(i).quat.x()));
		m_pTable->setItem(i,7,q1);
		q2 = new QTableWidgetItem(QString::number(m_Data.at(i).quat.y()));
		//q2.setText(QString::number(m_Data.at(i).quat.y()));
		m_pTable->setItem(i,8,q2);
		q3 = new QTableWidgetItem(QString::number(m_Data.at(i).quat.z()));
		//q3.setText(QString::number(m_Data.at(i).quat.z()));
		m_pTable->setItem(i,9,q3);
		//m_pTable->horizontalHeader()->setResizeMode(i,QHeaderView::ResizeToContents);
		//m_pTable->verticalHeader()->setResizeMode(i,QHeaderView::ResizeToContents);
	}
	m_pTable->setVerticalHeaderLabels(rows);
	m_pTable->resizeColumnsToContents();
	m_pTable->resizeRowsToContents();
	m_pTable->show();
	m_pTable->raise();

	delete xtran;
	delete ytran;
	delete ztran;
	delete xrot;
	delete yrot;
	delete zrot;
	delete q0;
	delete q1;
	delete q2;
	delete q3;
}

void JTSData::toQuaternion312( float z_rot, float x_rot, float y_rot, float* rotation )
{
	//UNIT: degree to radian
	float rr[3];    //Rotation in Radians
	rr[0] = z_rot /R2D;  // z
	rr[1] = x_rot /R2D;  // x
	rr[2] = y_rot /R2D;  // y

	//UNIT: radian
	//Rotation in Radians

	// suppose the rotation matrix is R.
	// The quaternion generated from this approach always has a non-negative
	//  scalar part (rotation[0]).
	float c1 = cos(rr[0]);
	float s1 = sin(rr[0]);
	float c2 = cos(rr[1]);
	float s2 = sin(rr[1]);
	float c3 = cos(rr[2]);
	float s3 = sin(rr[2]);
	float R11 = c1*c3-s1*s2*s3;
	float R22 = c1*c2;
	float R33 = c2*c3;
	float trace = R11+R22+R33;   // = 4coshalf^2-1 = 2cosine+1
	if (trace<-1) trace = -1;

	// avoid certain compiler optimization error and/or potential rounding error,
	// see 'case 312' in GetPose() for more info.
	// Actual bug case encountered here.
	volatile float coshalf_tmp = sqrt((trace+1)/4); // >=0. scalar part of the quaternion.
	float coshalf = coshalf_tmp;
	// 'volatile' to avoid potential compiler optimization error,
	// see 'case 312' in GetPose() for more info.
	float R12 = -s1*c2;
	float R13 = c1*s3+s1*s2*c3;
	float R21 = s1*c3+c1*s2*s3;
	float R23 = s1*s3-c1*s2*c3;
	float R31 = -c2*s3;
	float R32 = s2;
	rotation[0] = coshalf;  // my q0
	if (coshalf==0)  //180deg rotation
	{
		// For 180deg rotation, two opposite axes are possible. We make the
		// 1st element of the vector part always positive (unless it's zero,
		// in which case we will make the 2nd element positive, or the 3rd).

		volatile float tmp = sqrt( R22+R33>0 ? 0 : -(R22+R33)/2 );
		// 'volatile' to avoid potential compiler optimization error,
		// see 'case 312' in GetPose() for more info.
		// '?:' to avoid potential compiler optimization error and/or rounding error.
		rotation[1] = tmp;
		if (rotation[1]==0)
		{
			tmp = sqrt( R11+R33>0 ? 0 : -(R11+R33)/2 );
			// '?:' to avoid potential compiler optimization error and/or rounding error.
			rotation[2] = tmp;
			if (rotation[2]==0)
				rotation[3] = sqrt( R11+R22>0 ? 0 : -(R11+R22)/2 );
			// '?:' to avoid potential compiler optimization error and/or rounding error.
			else
				rotation[3] = R32/rotation[2]/2;
		}
		else
		{
			rotation[2] = R21/rotation[1]/2;
			rotation[3] = R31/rotation[1]/2;
		}
	}
	else
	{
		rotation[1] = (R32-R23)/4/coshalf;
		rotation[2] = (R13-R31)/4/coshalf;
		rotation[3] = (R21-R12)/4/coshalf;
	}
}